#include "FOC_DEF.h"
#include "FOC.h"
#include "Config.h"
#include "Clark.h"
#include "Park.h"
extern FOC::sMotor Motor_CH1, Motor_CH2;
void FOC::motorInit(void){
    
    pinMode(FOC_CH1_A, OUTPUT);
    ledcSetup(FOC_CH1_CH1, 5 * 100, 12);
    ledcAttachPin(FOC_CH1_A, FOC_CH1_CH1);
    pinMode(FOC_CH1_B, OUTPUT);
    ledcSetup(FOC_CH1_CH2, 5 * 100, 12);
    ledcAttachPin(FOC_CH1_B, FOC_CH1_CH2);
    pinMode(FOC_CH1_Z, OUTPUT);
    ledcSetup(FOC_CH1_CH3, 5 * 100, 12);
    ledcAttachPin(FOC_CH1_Z, FOC_CH1_CH3);

    pinMode(FOC_CH1_EN,OUTPUT);
    digitalWrite(FOC_CH1_EN,HIGH);
 
    pinMode(FOC_CH2_A, OUTPUT);
    ledcSetup(FOC_CH2_CH1, 5 * 10000, 12);
    ledcAttachPin(FOC_CH2_A, FOC_CH2_CH1);
    pinMode(FOC_CH2_B, OUTPUT);
    ledcSetup(FOC_CH2_CH2, 5 * 10000, 12);
    ledcAttachPin(FOC_CH2_B, FOC_CH2_CH2);
    pinMode(FOC_CH2_Z, OUTPUT);
    ledcSetup(FOC_CH2_CH3, 5 * 10000, 12);
    ledcAttachPin(FOC_CH2_Z, FOC_CH2_CH3);

    pinMode(FOC_CH2_EN,OUTPUT);
    digitalWrite(FOC_CH2_EN,HIGH);
    FOC::setTwoChannelSpeed(500,500);
    Serial.println("motor init successed!");
}

void FOC::setCH1Speed(int speed){
    Motor_CH1.q = 2048;
    Motor_CH1.motor_speed = speed;
    
}
void FOC::setCH2Speed(int speed){
    Motor_CH2.q = 2048;
    Motor_CH2.motor_speed = speed;
}
void FOC::setTwoChannelSpeed(int CH1_speed,int CH2_speed){
    FOC::setCH1Speed(CH1_speed);
    FOC::setCH2Speed(CH2_speed);
}
void FOC::CH1SpeedCtrl(void){
    if (Motor_CH1.dir)    Motor_CH1.c = Motor_CH1.c + Motor_CH1.motor_speed/10000.0;
    else Motor_CH1.c = Motor_CH1.c - Motor_CH1.motor_speed/10000.0;
    if (Motor_CH1.c >= 6.28) Motor_CH1.c = 0; 
    // Serial.println(c);
    float alpha = iParkToAlpha(Motor_CH1.d, Motor_CH1.q, Motor_CH1.c);
    float beta = iParkToBeta(Motor_CH1.d, Motor_CH1.q, Motor_CH1.c);
    Motor_CH1.A = iClarkAlphaBetaToA(alpha, beta);
    Motor_CH1.B = iClarkAlphaBetaToB(alpha, beta);
    Motor_CH1.C = iClarkAlphaBetaToC(alpha, beta);
    // if(A < 0) A = 0;
    // if(B < 0) B = 0;
    // if(C < 0) C = 0;
    ledcWrite(FOC_CH1_CH1,int(Motor_CH1.q+Motor_CH1.A)); //设置输出PWM
    ledcWrite(FOC_CH1_CH2,int(Motor_CH1.q+Motor_CH1.B)); //设置输出PWM
    ledcWrite(FOC_CH1_CH3,int(Motor_CH1.q+Motor_CH1.C)); //设置输出PWM   
    // ledcWrite(FOC_CH1_CH1,255); //设置输出PWM
    // ledcWrite(FOC_CH1_CH2,0); //设置输出PWM
    // ledcWrite(FOC_CH1_CH3,255); //设置输出PWM   
}
void FOC::CH2SpeedCtrl(void){
    if (Motor_CH2.dir)    Motor_CH2.c = Motor_CH2.c + Motor_CH2.motor_speed/10000.0;
    else Motor_CH2.c = Motor_CH2.c - Motor_CH2.motor_speed/10000.0;
    if (Motor_CH2.c >= 6.28) Motor_CH2.c = 0; 
    // Serial.println(c);
    float alpha = iParkToAlpha(Motor_CH2.d, Motor_CH2.q, Motor_CH2.c);
    float beta = iParkToBeta(Motor_CH2.d, Motor_CH2.q, Motor_CH2.c);
    Motor_CH2.A = iClarkAlphaBetaToA(alpha, beta);
    Motor_CH2.B = iClarkAlphaBetaToB(alpha, beta);
    Motor_CH2.C = iClarkAlphaBetaToC(alpha, beta);
    // if(A < 0) A = 0;
    // if(B < 0) B = 0;
    // if(C < 0) C = 0;
    ledcWrite(FOC_CH2_CH1,int(Motor_CH2.q+Motor_CH2.A)); //设置输出PWM
    ledcWrite(FOC_CH2_CH2,int(Motor_CH2.q+Motor_CH2.B)); //设置输出PWM
    ledcWrite(FOC_CH2_CH3,int(Motor_CH2.q+Motor_CH2.C)); //设置输出PWM

    // ledcWrite(FOC_CH2_CH1,255); //设置输出PWM
    // ledcWrite(FOC_CH2_CH2,0); //设置输出PWM
    // ledcWrite(FOC_CH2_CH3,255); //设置输出PWM  
}

void FOC::changeTone(int Tone){
    ledcSetup(FOC_CH1_CH1, Tone, 12);
    ledcSetup(FOC_CH1_CH2, Tone, 12);
    ledcSetup(FOC_CH1_CH3, Tone, 12);
}